import imufusion
import matplotlib.pyplot as pyplot
import numpy
import sys
import math
import matplotlib.pyplot as plt

data0 = numpy.genfromtxt("C:/Users/xiech/Desktop/acc.csv", delimiter=",", skip_header=1)
data1 = numpy.genfromtxt("C:/Users/xiech/Desktop/gyro.csv", delimiter=",", skip_header=1)
data2=numpy.genfromtxt("C:/Users/xiech/Desktop/time.csv", delimiter=",", skip_header=1)

accelerometer = data0[0:640, 0:3]
gyroscope = data1[0:640, 0:3]
timestamp=data2[0:640]
accelerometer1=accelerometer/9.8
gyroscope1=gyroscope/math.pi*180

# Plot sensor data
_, axes = pyplot.subplots(nrows=3, sharex=True)

axes[1].plot(timestamp, gyroscope1[:, 0], "tab:red", label="X")
axes[1].plot(timestamp, gyroscope1[:, 1], "tab:green", label="Y")
axes[1].plot(timestamp, gyroscope1[:, 2], "tab:blue", label="Z")
axes[1].set_title("Gyroscope")
axes[1].set_ylabel("Degrees/s")
axes[1].grid()
axes[1].legend()

axes[0].plot(timestamp, accelerometer1[:, 0], "tab:red", label="X")
axes[0].plot(timestamp, accelerometer1[:, 1], "tab:green", label="Y")
axes[0].plot(timestamp, accelerometer1[:, 2], "tab:blue", label="Z")
axes[0].set_title("Accelerometer")
axes[0].set_ylabel("g")
axes[0].grid()
axes[0].legend()

# Process sensor data
ahrs = imufusion.Ahrs()
euler = numpy.empty((640, 3))

for index in range(640):
    ahrs.update_no_magnetometer(gyroscope1[index], accelerometer1[index], 1 / 50)  # 50 Hz sample rate
    euler[index] = ahrs.quaternion.to_euler()

#Plot Euler angles
axes[2].plot(timestamp, -euler[:, 0], "tab:red", label="Roll")
axes[2].plot(timestamp, -euler[:, 1], "tab:green", label="Pitch")
axes[2].plot(timestamp, -euler[:, 2], "tab:blue", label="Yaw")
axes[2].set_title("Euler angles")
axes[2].set_xlabel("Seconds")
axes[2].set_ylabel("Degrees")
axes[2].grid()
axes[2].legend()

if len(sys.argv) == 1:  # don't show plots when script run by CI
    pyplot.show()


plt.figure()
x=0
y=0
vx=0
vy=0
l=0
a=0
v=0
f=open("C:/Users/xiech/Desktop/v.csv","a")
for i in range(100,550):
    if(abs(accelerometer[i][1])<=1):
        a=0
    else:
        a=-accelerometer[i][1]
    v=v+a*0.02
    l=-euler[i][2]/180*math.pi
    vx=-v*math.sin(l)
    vy=v*math.cos(l)
    x=vx*0.02+x
    y=vy*0.02+y
    f.write(str(x)+","+str(y)+"\n")
    plt.scatter(x,  # 横坐标
        y,  # 纵坐标
        c='red',  # 点的颜色
                )

plt.scatter(0,  0,  c='blue'  )              
plt.scatter(x,  y,  c='green'  )
f.close()
ax = plt.gca()
ax.set_aspect(1)
plt.show()  # 显示所绘图形


_, axes0 = pyplot.subplots(nrows=1, sharex=True)
data3=numpy.genfromtxt("C:/Users/xiech/Desktop/v.csv", delimiter=",", skip_header=1)
axes0.plot(timestamp[100:550,], data3[0:450,0], "tab:red", label="vx")
axes0.plot(timestamp[100:550,], data3[0:450,1], "tab:green", label="vy")
axes0.plot(timestamp[100:550,], data3[0:450,2], "tab:blue", label="v")
axes0.set_title("Speed of movement")
axes0.set_xlabel("Seconds")
axes0.set_ylabel("m/s")
axes0.grid()
axes0.legend()


if len(sys.argv) == 1:  # don't show plots when script run by CI
    pyplot.show()